clc
clear
close all

addpath('mpc','simulace');

%% inicializace

Ts=0.1;
Tref=30;
Tmax=40;

refPomoc=zeros(2,Tref/Ts);

refPomoc(1,(Tref/0.4):(3*Tref/0.4))=-80;
refPomoc(2,(Tref/0.2):(Tref/0.1))=-80;

for i=1:length(refPomoc);
    ref(2*i-1)=refPomoc(1,i);
    ref(2*i)=refPomoc(2,i);
end


param.ref=ref;

%% simulace1
param.Q=100;
param.R=1;
param.H=100;
param.hard_lb=[-100;-100];
param.hard_ub=[100;100];
param.soft_lb=[-2;-2];
param.soft_ub=[2;2];
param.wApply=1:2:20;
param.Tvz=5;
param.Block=[4 4 4 4 4];


[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft_param,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;
u=out.u;

%% vykresleni

figure
subplot(2,2,1)
hold on
title('y 1')
plot(t,ref(1,:),'r');
plot(t,y(1,:));
plot(t,ref(1,:)+param.soft_lb(1),'y');
plot(t,ref(1,:)+param.soft_ub(1),'g');
grid on

subplot(2,2,3)
hold on
title('y 2')
plot(t,u(1,:));
plot(t,ones(size(t))*param.hard_lb(1),'y');
plot(t,ones(size(t))*param.hard_ub(1),'g');
grid on

subplot(2,2,2)
hold on
title('u 1');
plot(t,ref(2,:),'r');
plot(t,y(2,:));
plot(t,ref(2,:)+param.soft_lb(2),'y');
plot(t,ref(2,:)+param.soft_ub(2),'g');
grid on

subplot(2,2,4)
hold on
title('u 2');
plot(t,u(2,:));
plot(t,ones(size(t))*param.hard_lb(2),'y');
plot(t,ones(size(t))*param.hard_ub(2),'g');
grid on